#include <libpub.h>
#include <xllp_gpio.h>
#include <xllp_defs.h>

#define PGPIO_REG_SET   ((P_XLLP_GPIO_T) 0x40e00000)

static void dump_gpio()
{
    printf("\nGPLR0 = 0X%08X", PGPIO_REG_SET->gplr0);
    printf("\nGPLR1 = 0X%08X", PGPIO_REG_SET->gplr1);
    printf("\nGPLR2 = 0X%08X", PGPIO_REG_SET->gplr2);
    printf("\nGPLR3 = 0X%08X", PGPIO_REG_SET->gplr3);
}

static inline int check_port(int port)
{
#define PORT_MAX 127
#define PORT_MIN 0
    if(port < PORT_MIN || port > PORT_MAX){
        TOUT(1, 1, printf("\nError: port wrong"));
        return -1;
    }
    return 0;
}

static inline int check_port_set(int port_start, int port_end)
{
    int ret = 0;

    ret = check_port(port_start);
    ret += check_port(port_end);
    ret += ((port_start > port_end) ? -1 : 0);

    return ((ret < 0) ? -1 : 0);
}


static inline int gpio_dump_one_port(int port)
{
    TOUT(0, 1, printf("\ngpio_dump_one_port(%d)", port));
    XLLP_STATUS_T ret;

    XLLP_LEVEL_T  level;//To Store the Result
    XLLP_UINT32_T gpio_pin = (XLLP_UINT32_T) port;

    if(-1 == check_port(port)) {
        printf("\n Port Illegal...");
        return -1;
    }

    ret = XllpGpioGetLevel(PGPIO_REG_SET, gpio_pin, &level);
    if(XLLP_STATUS_SUCCESS != ret){
        TOUT(1, 1, printf("\nGet GPIO level Wrong..."));
        return -1;
    }
    switch(level){
        case XLLP_HI:
            printf("\nPin: %d = High Level", port);
            return 1;

        case XLLP_LO:
            printf("\nPin: %d = Low Level", port);
            return 0;

        default:
            printf("\nWrong ... ");
            return -1;
    }
}

static inline int print_port(int *port_level_set, int port_start, int port_end, int level)
{
    int i, j = 0;
    printf("\n\nPort Level = %d:\n", level);
    for(i = port_start; i <= port_end; i++){
        if(level == port_level_set[i]){
            if(0 == (j++ % 8)) printf("\n");
            printf(" %4d ", i);
        }
    }
    return 0; 
}

static int print_low_level_port(int *port_level_set, int port_start, int port_end)
{
    print_port(port_level_set, port_start, port_end, 0);
    return 0;
}

static int print_high_level_port(int *port_level_set, int port_start, int port_end)
{
    print_port(port_level_set, port_start, port_end, 1);
    return 0;
}

static inline int gpio_dump_port_set(int port_start, int port_end)
{
    TOUT(1, 1, printf("\nport_start = %d, port_end = %d", port_start, port_end ));
    int port = 0;
#define PORT_NUM 128
    int port_level_set[PORT_NUM];//0:Low Level 1:High Level
    int low_level_port_cnt = 0;
    int high_level_port_cnt = 0;
    int wrong_level_port_cnt = 0;
    int total_cnt = 0;

    if(-1 == check_port_set(port_start, port_end)){
        printf("\nport set error");
        return -1;
    }

    printf("\nCheck Port From:%d --> %d", port_start, port_end);
    for(port = port_start; port <= port_end; port++){
        switch(gpio_dump_one_port(port)){
            case 0://Low Level
                TOUT(0, 1, printf("\nport = Low Level", port));
                port_level_set[port] = 0;
                low_level_port_cnt++;
                total_cnt++;
                break;

            case 1://High Level
                TOUT(0, 1, printf("\nport = High Level", port));
                port_level_set[port] = 1;
                high_level_port_cnt++;
                total_cnt++;
                break;

            case -1:
                TOUT(1, 1, printf("\nset %d port level wrong...", port));
                port_level_set[port] = -1;
                wrong_level_port_cnt++;
                total_cnt++;
                break;
        }
    }
    printf("\n\nTotal Ports:%d [%d,%d]", total_cnt, port_start, port_end);
    printf("\nLow Level Ports: %d", low_level_port_cnt);
    printf("\nHigh Level Ports: %d", high_level_port_cnt);
    print_low_level_port(port_level_set, port_start, port_end);
    print_high_level_port(port_level_set, port_start, port_end);

    return 0;
}

static inline int gpio_set_port_level(int port, int level)
{
    XLLP_STATUS_T ret = XLLP_STATUS_FAILURE;
    XLLP_LEVEL_T pin_level;
    XLLP_UINT32_T gpio_pin = (XLLP_UINT32_T)port;

    level = ((level == 0) ? 0 : 1);//Restrict level to 0 or 1
    pin_level = ((level == 0) ? XLLP_LO : XLLP_HI);

    if(-1 == check_port(port)){
        printf("\nError port illegal");
        return -1;
    }
    XllpGpioSetDirection(PGPIO_REG_SET, gpio_pin, XLLP_GPIO_DIRECTION_OUT);
    TOUT(1, 1, printf("gpio_set_port_level(%d, %d)", port, level));
    ret = XllpGpioSetOutputLevel(PGPIO_REG_SET, gpio_pin, pin_level);
    if(XLLP_STATUS_SUCCESS != ret){
        printf("\n Pin Set Wrong");
        return -1;
    }
    TOUT(1, 1, printf("\nOK set gpio %d  = %d", port, pin_level));
    dump_gpio();

    return 0;
}

static int gpio_set_port_0(int port)
{
    gpio_set_port_level(port, 0);
    return 0;
}

static inline int gpio_set_port_1(int port)
{
    gpio_set_port_level(port, 1);
    return 0;
}

//Usage:
//gpio dump
//gpio dump .18   gpio dump 18
//gpio set .19 1  gpio set .19 0
//gpio set 23 1   gpio set 23 0
int _Do_Test_Gpio(char *param)
{
    TOUT(1, 1, printf("\n_Do_TEST_Gpio(%s)", param));
    int port = 0;
    int port_start = 0 , port_end = 0;
    int level = 0;
    char *sub = NULL;

    if(*param){//gpio
        param = Bo_ParseString(param, &sub);
        if(0 == Bo_MatchString(sub, "set")) {//gpio set
            TOUT(1, 1, printf("\ngpio set"));
            param = Bo_ParseString(param, &sub);
            port = Bo_ConvertString(sub);//gpio set port
            TOUT(1, 1, printf("\ngpio set %d", port));
            if(port < PORT_MIN || port > PORT_MAX){
                TOUT(1, 1, printf("\nError:port wrong"));
                return -1;
            }
            param = Bo_ParseString(param, &sub);
            level = Bo_ConvertString(sub);//gpio set port level:1:High 0:Low
            if(0 == level){//gpio set port 0
                TOUT(1, 1, printf("\ngpio set %d 0", port));
                //Todo gpio set port 0
                gpio_set_port_0(port);
            }
            else if(1 == level){//gpio set port 1
                TOUT(1, 1, printf("\ngpio set %d 1", port));
                //Todo gpio set port 1
                gpio_set_port_1(port);
            }
            else{
                TOUT(1, 1, printf("\ngpio set %d %d", port, level));
                return -1;
            }
        }
        else if (0 == Bo_MatchString(sub, "dump")){ //gpio dump
            TOUT(0, 1, printf("\ngpio dump:"));
            if(*param){//gpio dump port1
                TOUT(0, 1, printf("\nenter here:"));
                param = Bo_ParseString(param, &sub);
                port_start = Bo_ConvertString(sub);
                if(check_port(port_start) == -1)
                    return -1;
                if(*param){//gpio dump port1 port2
                    param = Bo_ParseString(param, &sub);
                    port_end = Bo_ConvertString(sub);
                    if(check_port(port_end) == -1)
                        return -1;
                    //Todo: gpio dump port1 port2
                    TOUT(1, 1, printf("\n==>gpio dump %d %d", port_start, port_end));
                    gpio_dump_port_set(port_start, port_end);
                }
                else{
                    //ToDo : gpio dump port
                    TOUT(1, 1, printf("\n==>gpio dump %d", port_start));
                    gpio_dump_one_port(port_start);
                }
            }
            else{
                TOUT(1, 1, printf("\n==>gpio dump"));
                //Todo gpio dump ,dump all the gpios here:
                gpio_dump_port_set(PORT_MIN, PORT_MAX);
                printf("\n");
            }

        }
        else{
            TOUT(1, 1, printf("\nWrong Input"));
            return -1;
        }

    }
    else{
        TOUT(1, 1, printf("\nWrong Input"));
        return -1;
    }

    return 0;
}
